{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys \n",
    "sys.path.append('../scripts/')\n",
    "from kf import *   #誤差楕円を描くのに利用"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw(xs, Rs):  ###graphslam3draw\n",
    "    ##世界の描画##\n",
    "    fig = plt.figure(figsize=(4,4))\n",
    "    ax = fig.add_subplot(111) \n",
    "    ax.set_aspect('equal')\n",
    "    ax.set_xlim(-5,5)\n",
    "    ax.set_ylim(-5,5) \n",
    "    ax.set_xlabel(\"X\",fontsize=10) \n",
    "    ax.set_ylabel(\"Y\",fontsize=10)  \n",
    "    \n",
    "    ##軌跡の描画##\n",
    "    poses = [xs[s] for s in range(len(hat_xs))]\n",
    "    ax.scatter([e[0] for e in poses], [e[1] for e in poses], s=5, marker=\".\", color=\"black\")\n",
    "    ax.plot([e[0] for e in poses], [e[1] for e in poses], linewidth=0.5, color=\"black\")\n",
    "    \n",
    "    ##R_tの描画##  #追加\n",
    "    for t in range(1,t_end+1):\n",
    "        ax.add_patch( sigma_ellipse(xs[t], Rs[t], 3) )\n",
    "    \n",
    "    ##描画実行##\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "###データの読み込み###\n",
    "delta = 0.0\n",
    "xs = {}     #軌跡のデータ（ステップ数をキーにして姿勢を保存）\n",
    "us = {}    #制御入力のデータ（ステップ数をキーにして保存）\n",
    "\n",
    "with open(\"log.txt\") as f:\n",
    "    for line in f.readlines():\n",
    "        tmp = line.rstrip().split()\n",
    "        \n",
    "        step = float(tmp[1])\n",
    "        if tmp[0] == \"delta\":\n",
    "            delta = float(tmp[1])\n",
    "        elif tmp[0] == \"x\": #姿勢のレコードの場合\n",
    "            xs[step] = np.array([float(tmp[2]), float(tmp[3]), float(tmp[4])]).T\n",
    "        elif tmp[0] == \"u\": #制御入力の場合\n",
    "            us[step] = np.array([float(tmp[2]), float(tmp[3])]).T\n",
    "            \n",
    "import copy\n",
    "hat_xs = copy.copy(xs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "41\n",
      "[[0. 0. 0. ... 0. 0. 0.]\n",
      " [0. 0. 0. ... 0. 0. 0.]\n",
      " [0. 0. 0. ... 0. 0. 0.]\n",
      " ...\n",
      " [0. 0. 0. ... 0. 0. 0.]\n",
      " [0. 0. 0. ... 0. 0. 0.]\n",
      " [0. 0. 0. ... 0. 0. 0.]]\n",
      "[[0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]\n",
      " [0.]]\n"
     ]
    }
   ],
   "source": [
    "##空の精度行列と一次の項のベクトルの準備## \n",
    "t_end = len(xs) - 1 #step数（数式中のT）\n",
    "#t_end = 1\n",
    "\n",
    "##精度行列の次元を求めて精度行列と一次の項のベクトルの初期化##\n",
    "dim = (t_end + 1)*3# + len(observed_landmarks)*2\n",
    "Omega = np.zeros((dim, dim))\n",
    "xi = np.zeros((dim, 1))\n",
    "\n",
    "##確認##\n",
    "print(t_end)                           #終了時刻\n",
    "print(Omega)                        #精度行列\n",
    "print(xi)                                  #一次の項のベクトル"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Ω0 =  [[1000000.       0.       0.]\n",
      " [      0. 1000000.       0.]\n",
      " [      0.       0. 1000000.]]\n",
      "ξ0 = [       0. -3000000.        0.]\n"
     ]
    }
   ],
   "source": [
    "##Ω_0を作る## ###graphslam3omzero\n",
    "alpha = 0.001\n",
    "Sigma_0 = np.diag([0.001**2, 0.001**2, 0.001**2])\n",
    "\n",
    "Om_0 = np.linalg.inv(Sigma_0) #3x3の精度行列としてΩ0を作る\n",
    "xi_0 = Om_0.dot(xs[0])\n",
    "\n",
    "print(\"Ω0 = \", Om_0)\n",
    "print(\"ξ0 =\", xi_0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "ename": "KeyError",
     "evalue": "4",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyError\u001b[0m                                  Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-6-ebc81c5ac5a1>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     35\u001b[0m \u001b[0mRs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mFs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mRinvs\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m{\u001b[0m\u001b[0;34m}\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m{\u001b[0m\u001b[0;34m}\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m{\u001b[0m\u001b[0;34m}\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     36\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mt\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mt_end\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 37\u001b[0;31m     \u001b[0mRs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mR\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     38\u001b[0m     \u001b[0mRinvs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlinalg\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0minv\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mRs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     39\u001b[0m     \u001b[0mFs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mF\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-6-ebc81c5ac5a1>\u001b[0m in \u001b[0;36mR\u001b[0;34m(t)\u001b[0m\n\u001b[1;32m      4\u001b[0m \u001b[0;32mdef\u001b[0m \u001b[0mR\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0;31m#R_tを返す\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      5\u001b[0m     \u001b[0mtheta\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mxs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 6\u001b[0;31m     \u001b[0mnu\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mom\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mus\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      7\u001b[0m \u001b[0;31m#    if abs(om) < 1e-5: om = 1e-5 #ゼロにすると式が変わるので避ける\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      8\u001b[0m \u001b[0;31m#    if abs(nu) < 1e-5:         nu = 1e-5\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mKeyError\u001b[0m: 4"
     ]
    }
   ],
   "source": [
    "##Ωxx,ξxxを作る##   ###graphslam3motioninfo\n",
    "motion_noise_stds = {\"nn\":0.19, \"no\":0.001, \"on\":0.13, \"oo\":0.2}\n",
    "\n",
    "def R(t): #R_tを返す\n",
    "    theta = xs[t-1][2]\n",
    "    nu, om = us[t]\n",
    "#    if abs(om) < 1e-5: om = 1e-5 #ゼロにすると式が変わるので避ける\n",
    "#    if abs(nu) < 1e-5:         nu = 1e-5\n",
    "        \n",
    "    st, ct = math.sin(theta), math.cos(theta)\n",
    "    stw, ctw = math.sin(theta + om*delta), math.cos(theta + om*delta)\n",
    "    A = np.array([[(stw - st)/om,    -nu/(om**2)*(stw - st) + nu/om*delta*ctw],\n",
    "                                 [(-ctw + ct)/om, -nu/(om**2)*(-ctw + ct) + nu/om*delta*stw],\n",
    "                                 [0,                                delta]] )\n",
    "    M = np.diag(\n",
    "        [motion_noise_stds[\"nn\"]**2*abs(nu)/delta +motion_noise_stds[\"no\"]**2*abs(om)/delta,\n",
    "             motion_noise_stds[\"on\"]**2*abs(nu)/delta + motion_noise_stds[\"oo\"]**2*abs(om)/delta]\n",
    "    )\n",
    "    return A.dot(M).dot(A.T) + np.diag([0.01**2, 0.01**2, 0.01**2]) #xyθ方向に誤差を混ぜておく\n",
    "    #return np.diag([0.1**2, 0.1**2, 0.1**2]) \n",
    "\n",
    "def F(t): #F_x_{t-1}を返す（添字注意）\n",
    "    F = np.array(np.eye(3))\n",
    "    theta = xs[t-1][2]\n",
    "    nu, om = us[t-1]\n",
    "#    if abs(om) < 1e-5: om = 1e-5 #ゼロにすると式が変わるので避ける\n",
    "#    if abs(nu) < 1e-5:         nu = 1e-5\n",
    "    \n",
    "    F[0, 2] = nu / om * (math.cos(theta + om * delta) - math.cos(theta))\n",
    "    F[1, 2] = nu / om * (math.sin(theta + om * delta) - math.sin(theta))\n",
    "        \n",
    "    return F\n",
    "\n",
    "##必要な行列を先に計算##\n",
    "Rs, Fs, Rinvs = {}, {}, {}\n",
    "for t in range(1, t_end+1):\n",
    "    Rs[t] = R(t)\n",
    "    Rinvs[t] = np.linalg.inv(Rs[t])\n",
    "    Fs[t-1] = F(t)\n",
    "    \n",
    "##各x_{t-1}, x_tペアの精度行列と一次の項のベクトルを作る##\n",
    "Omxx_ul, Omxx_ur, Omxx_bl, Omxx_br = {}, {}, {}, {} #x_{t-1}とx_tに対する精度行列を保管する辞書\n",
    "xixx_u, xixx_b = {}, {}\n",
    "\n",
    "import copy\n",
    "\n",
    "for t in range(1, t_end+1):\n",
    "    Omxx_ul[(t-1, t)] = Fs[t-1].T.dot(Rinvs[t]).dot(Fs[t-1])\n",
    "    Omxx_ur[(t-1, t)] = -Fs[t-1].T.dot(Rinvs[t])\n",
    "    Omxx_bl[(t-1, t)] = -Rinvs[t].dot(Fs[t-1].T)\n",
    "    Omxx_br[(t-1, t)] = copy.copy(Rinvs[t])\n",
    "    \n",
    "    xixx_b[(t-1, t)] = np.array([0,0,0])#Rinvs[t].dot(hat_xs[t]- Fs[t-1].dot(xs[t-1])) #下の3行から計算\n",
    "    xixx_u[(t-1, t)] = np.array([0,0,0])#- Fs[t-1].T.dot(xixx_b[(t-1, t)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Omega = np.zeros((dim, dim))\n",
    "xi = np.zeros((dim, 1))\n",
    "\n",
    "Omega[0:3, 0:3] += Om_0\n",
    "xi[0:3, 0] += xi_0\n",
    "\n",
    "for t in range(1, t_end+1):\n",
    "    Omega[(t-1)*3:t*3, (t-1)*3:t*3] +=  Omxx_ul[(t-1, t)]\n",
    "    Omega[(t-1)*3:t*3, t*3:(t+1)*3] +=  Omxx_ur[(t-1, t)]\n",
    "    Omega[t*3:(t+1)*3, (t-1)*3:t*3] +=  Omxx_bl[(t-1, t)]\n",
    "    Omega[t*3:(t+1)*3, t*3:(t+1)*3] +=  Omxx_br[(t-1, t)]\n",
    "    \n",
    "    xi[(t-1)*3:t*3, 0] += xixx_u[(t-1, t)]\n",
    "    xi[t*3:(t+1)*3, 0] += xixx_b[(t-1, t)]\n",
    "    \n",
    "#xx = np.zeros((dim, 1))\n",
    "#for t in range(t_end+1):\n",
    "#    xx[t*3:t*3+3, 0] += xs[t]\n",
    "#xi = Omega.dot(xx) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = Omega[0:(t_end+1)*3 , 0:(t_end+1)*3 ]\n",
    "b = xi[0:(t_end+1)*3, 0]\n",
    "\n",
    "#A = Omega[0:6 , 0:6 ]\n",
    "#b = xi[0:6, 0]\n",
    "\n",
    "print(hat_xs[1])\n",
    "\n",
    "c = np.linalg.inv(A).dot(b)\n",
    "for t in range(1,t_end+1):\n",
    "    hat_xs[t] = c[t*3:t*3+3]\n",
    "    \n",
    "print(hat_xs[1])\n",
    "    \n",
    "draw(hat_xs, Rs)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
